using System;
using System.Collections.Generic;

namespace Diversity.Robotics.Navigation.GridSlamApp
{
    /// <summary>
    /// MOST OF THIS ISN'T NEEDED as many of the functions have been replaced by more generic
    /// varieties in prob utils.
    /// 
    /// Don't really want the concept of a sample any more. Prefer states and weights to be seperate.
    /// </summary>

    public class VelocityMotionParticleFilter
    {
        private VelocityMotionModel _vmm;
        private int _numSamples;
        private Random _random;
        public Pose2D _perfectState;

        public VelocityMotionParticleFilter(VelocityMotionModel vmm, int numSamples)
        {
            _random = new Random(DateTime.Now.Millisecond);
            _vmm = vmm;
            _numSamples = numSamples;
        }

        public List<DifferentialDriveStateSample> CreateStartSamples(Pose2D state)
        {
            List<DifferentialDriveStateSample> Samples = new List<DifferentialDriveStateSample>();
            for(int i=0; i<NumSamples; i++)
            {
                Samples.Add(new DifferentialDriveStateSample(state, 1.0 /NumSamples));
            }
            _perfectState = (Pose2D)state.Clone();
            return Samples;
        }

        public VelocityMotionModel Vmm
        {
            get { return _vmm; }
            set { _vmm = value; }
        }

        public int NumSamples
        {
            get { return _numSamples; }
            set { _numSamples = value; }
        }

        public List<DifferentialDriveStateSample> GetSamples(List<DifferentialDriveStateSample> previous, Velocity vel, TimeSpan t, out double totalProbability)
        {
            DateTime t0 = DateTime.Now;
            List<DifferentialDriveStateSample> Samples = new List<DifferentialDriveStateSample>();
            totalProbability = 0;
            Vmm.PrepareForSampling(vel, t);
            _perfectState = Vmm.GetPerfectSample(_perfectState, vel, t);
            for (int i = 0; i < previous.Count; i++)
            {
                Pose2D state = Vmm.GetSample(previous[i].State);
                double weight = Vmm.GetProbabilityOfSample(state, previous[i].State);
                Samples.Add(new DifferentialDriveStateSample(state,weight));
                totalProbability += weight;
            }
            DateTime t1 = DateTime.Now;
            Console.Out.WriteLine("Sampled " + NumSamples + " particles in " + TimeSpan.FromTicks(t1.Ticks - t0.Ticks).TotalSeconds);
            return Samples;
        }

        public List<DifferentialDriveStateSample> Normalize(List<DifferentialDriveStateSample> Samples, double CurrentTotal)
        {
            Console.Out.WriteLine("Before normalizing, total weight was " + CurrentTotal);
            double totalWeight = 0;
            foreach (DifferentialDriveStateSample s in Samples)
            {
                s.Weight = s.Weight / CurrentTotal;
                totalWeight += s.Weight;
            }
            Console.Out.WriteLine("After normalizing, total weight was " + totalWeight);
            return Samples;
        }
        
        public List<DifferentialDriveStateSample> ReSample(List<DifferentialDriveStateSample> Samples, out double totalProbability)
        {
            int numSamples = Samples.Count;
            totalProbability = 0;
            List<DifferentialDriveStateSample> ReturnSamples = new List<DifferentialDriveStateSample>();
            double r =  _random.NextDouble() / _numSamples;
            double c = Samples[0].Weight;
           // Console.Out.WriteLine("r: " + r + " weight: " + c);
            int i = 0;
            double U;
            for (int m=1; m<numSamples;m++)
            {
                
                if (m > 0)
                {
                    U = r + (m * 1.0) / numSamples;
                }
                else
                {
                    U = r;
                }
               //c = Samples[i].Weight;
                while (U > c )
                {
                    i++;
                    if (i >= numSamples -1)
                    {
                        // protect from overflow and return to start
                        i = 0;
                    }
                    c = c + Samples[i].Weight;
                    if (c == 0)
                    {
                        // there must be a problem
                        return ReturnSamples;
                    }
                   // Console.Out.WriteLine("inner loop c:" + c + " i:" + i + " U:" + U);
                }
              //  Console.Out.WriteLine("Adding sample " + i + " of weight: " + Samples[i].Weight + " Theta:" + Samples[i].State.Theta + " U:" + U + " c: " + c);
                ReturnSamples.Add(Samples[i]);
                totalProbability += Samples[i].Weight;
               // c = 0;
            }
            Console.Out.WriteLine("After resampling, total weight was " + totalProbability);
            return ReturnSamples;
        }
    }
}
